/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
*/

#include "printf_inc.h"
#include "VCan_Types.h"
#include "can_output.h"

#define TX_MB_CTRL_QUEUE_SIZE 16

struct can_tx_queue {
    void *items[TX_MB_CTRL_QUEUE_SIZE];
    int front;
    int rear;
};

static struct can_tx_queue tx_queue[LLCG_CONFIG_MAXCTRL_COUNT];


/**
 * @brief Initializes the send queue.
 */
void can_txque_init(void)
{
    for (int i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; ++i) {
        tx_queue[i].front = -1;
        tx_queue[i].rear = -1;
    }
}

/**
 * @brief Checks if the queue is empty.
 *
 * @param txque_idx The queue index.
 * @return true if the queue is empty, false otherwise.
 */
bool can_txque_is_empty(uint32 txque_idx)
{
    if (txque_idx >= LLCG_CONFIG_MAXCTRL_COUNT)
        return false;
    return tx_queue[txque_idx].front == -1;
}

/**
 * @brief Checks if the queue is full.
 *
 * @param txque_idx The queue index.
 * @return true if the queue is full, false otherwise.
 */
static bool can_txque_is_full(uint32 txque_idx)
{
    if (txque_idx >= LLCG_CONFIG_MAXCTRL_COUNT)
        return true;
    return (tx_queue[txque_idx].rear + 1) % TX_MB_CTRL_QUEUE_SIZE == tx_queue[txque_idx].front;
}

/**
 * @brief Enqueues an tx msg item into the control queue.
 *
 * @param txque_idx The control queue index.
 * @param x2can The X2CAN item to enqueue.
 * @return 0 on success, negative value on error.
 */
sint32 can_txque_enqueue(uint32 txque_idx, void *tx_msg)
{
    if (txque_idx >= LLCG_CONFIG_MAXCTRL_COUNT) {
        hprintf(ERROR, "Index of ctrl queue overflow. txque_idx=%d\n", txque_idx);
        return CAN_TX_ERR_PARA;
    }

    if (!tx_msg) {
        hprintf(ERROR, "Ctrl queue cannot enqueue NULL.\n");
        return CAN_TX_ERR_PARA;
    }

    if (can_txque_is_full(txque_idx)) {
        hprintf(ERROR, "Ctrl queue is full. Cannot enqueue. txque_idx=%d\n", txque_idx);
        return CAN_TX_ERR_QUEUE_FULL;
    }

    if (can_txque_is_empty(txque_idx)) {
        tx_queue[txque_idx].front = 0;
        tx_queue[txque_idx].rear = 0;
    } else {
        tx_queue[txque_idx].rear = (tx_queue[txque_idx].rear + 1) % TX_MB_CTRL_QUEUE_SIZE;
    }

    tx_queue[txque_idx].items[tx_queue[txque_idx].rear] = tx_msg;
    return 0;
}

/**
 * @brief Dequeues an tx msg item from the control queue.
 *
 * @param txque_idx The control queue index.
 * @param output The output X2CAN item.
 * @return 0 on success, negative value on error.
 */
sint32 can_txque_dequeue(uint32 txque_idx, void **tx_msg)
{
    if (txque_idx >= LLCG_CONFIG_MAXCTRL_COUNT) {
        hprintf(ERROR, "Index of ctrl queue overflow.\n");
        return CAN_TX_ERR_PARA;
    }

    if (can_txque_is_empty(txque_idx)) {
        // hprintf(ERROR, "Ctrl queue is empty. Cannot dequeue.\n");
        return CAN_TX_ERR_QUEUE_EMPTY;
    }

    *tx_msg = tx_queue[txque_idx].items[tx_queue[txque_idx].front];
    tx_queue[txque_idx].items[tx_queue[txque_idx].front] = NULL;
    if (tx_queue[txque_idx].front == tx_queue[txque_idx].rear) {
        tx_queue[txque_idx].front = -1;
        tx_queue[txque_idx].rear = -1;
    } else {
        tx_queue[txque_idx].front = (tx_queue[txque_idx].front + 1) % TX_MB_CTRL_QUEUE_SIZE;
    }

    return 0;
}
